#include <util/math_macros.h>

#include "../../imp_types.h"

#include "../../matrix/matrix.h"

#include "../../geometry/geometry.h"
#include "../../geometry/delaunay.h"

#include "../pointop/threshold.h"
#include "../pointop/nonmaxsup.h"
#include "../analysis/roi.h"
#include "../transforms/integral.h"

// =============================================================================

struct __triang_opt {
	boolean visited;
};

#define T_OPT(t) ((struct __triang_opt *)(IMP_OBJECT(t)->user_ptr))

struct __grid_point {
	boolean visited;

	int xw;
	int yw;

	real ix;
	real iy;
};

#define GRID(v) ((struct __grid_point *)(IMP_OBJECT(v)->user_ptr))

// <editor-fold defaultstate="collapsed" desc="Auxiliar Filt Chessboard">

/**
 * \brief Remove todos os pontos com menos de 2 vizinhos ou mais de 8 vizinhos
 * @param surf
 * @param changes
 */
static void __remove_invalid_points(ImpSurface * surf, int * changes) {

	ListIterator * it = list_get_first(surf->vertices);
	ImpVertex * v;

	(*changes) = 0;

	it = list_get_first(surf->vertices);
	while (it) {

		v = IMP_VERTEX(it->data);

		if (v->segments->size < 2 || v->segments->size > 8) {
			imp_surface_remove_vertex(surf, v);
			imp_object_delete(v);

			(*changes)++;
		}
		it = it->next;
	}
}

/**
 * \brief Removes all triangles by area.
 * @param surf
 * @param minArea
 * @param maxArea
 * @param changes
 */
static void __removes_triangles_by_area(ImpSurface * surf, real minArea, real maxArea, int * changes) {

	ListIterator * it = NULL;

	ImpTriangle * T;
	real a;

	(*changes) = 0;

	it = list_get_first(surf->faces);
	while (it) {

		T = IMP_TRIANGLE(it->data);
		it = it->next;

		a = imp_triangle_get_area(T);

		if (minArea > a || a > maxArea) {
			imp_surface_remove_triangle(surf, T);
			imp_object_delete(T);
			(*changes)++;
		}
	}
}

static boolean __triangle_has_color_transition(ImpTriangle * T, ImpMatrix * BW, real tx) {

	Ref2 p[3];
	Ref2 * ref;
	Ref2 bar;
	int idx;
	real val;
	boolean valid;

	// = Extract triangle points ===============================================
	ImpVertex * v1, * v2, * v3;
	imp_triangle_extract_vertices(T, &v1, &v2, &v3);

	p[0].x = IMP_POINT(v1)->x;
	p[0].y = IMP_POINT(v1)->y;

	p[1].x = IMP_POINT(v2)->x;
	p[1].y = IMP_POINT(v2)->y;

	p[2].x = IMP_POINT(v3)->x;
	p[2].y = IMP_POINT(v3)->y;

	// = Calculates internal triangle to avoid the borders =====================
	val = (p[0].x + p[1].x + p[2].x) / 3.0;
	bar.x = val;
	val = (p[0].y + p[1].y + p[2].y) / 3.0;
	bar.y = val;

	p[0].x = (bar.x - p[0].x) * tx + p[0].x;
	p[0].y = (bar.y - p[0].y) * tx + p[0].y;

	p[1].x = (bar.x - p[1].x) * tx + p[1].x;
	p[1].y = (bar.y - p[1].y) * tx + p[1].y;

	p[2].x = (bar.x - p[2].x) * tx + p[2].x;
	p[2].y = (bar.y - p[2].y) * tx + p[2].y;

	// = Get the points inside the internal triangle ===========================
	List * points = NULL;
	points = imp_image_roi_points(BW, p, 3);

	valid = FALSE; // returns TRUE by default

	ListIterator * it;
	it = list_get_first(points);

	ref = it->data;
	idx = imp_matrix_idx(BW, ref->x, ref->y);
	val = BW->data[idx];

	// = Verifies each point. If the value changes the current triangle is not valid.
	it = it->next;
	while (it) {

		ref = it->data;
		idx = imp_matrix_idx(BW, ref->x, ref->y);
		if (val != BW->data[idx]) {
			// = Invalid triangle ==============================================
			valid = TRUE;
			break;
		}
		it = it->next;
	}

	list_remove_all_data_full(points, free);

	return valid;
}

/**
 * \brief Removes all triangles with color transition inside.
 * @param surf
 * @param BW
 * @param tx
 * @param changes
 */
static void __removes_triangles_with_color_transition(ImpSurface * surf, ImpMatrix * BW, real tx, int * changes) {
	ListIterator * it = NULL;

	(*changes) = 0;

	it = list_get_first(surf->faces);
	while (it) {

		ImpTriangle * T;

		T = IMP_TRIANGLE(it->data);
		it = it->next;

		if (__triangle_has_color_transition(T, BW, tx)) {
			imp_surface_remove_triangle(surf, T);
			imp_object_delete(T);
			(*changes)++;
		}
	}
}

static int __count_triangle_neighbors_with_same_color(ImpTriangle * T, ImpMatrix * BW) {

	ImpTriangle * t = NULL;
	ImpPoint p;
	int i;

	real color_T, color_t;
	int nd;

	p = imp_triangle_barycenter(T);
	color_T = BW->data[ imp_matrix_idx(BW, p.x, p.y) ];


	nd = 0;
	for (i = 0; i < 3; i++) {
		t = imp_triangle_get_neighbor(T, T->e[i]);

		if (!t) continue;

		p = imp_triangle_barycenter(t);
		color_t = BW->data[ imp_matrix_idx(BW, p.x, p.y) ];

		if (color_T == color_t) nd++;
	}
	return nd;
}

/**
 * \brief Removes all triangles with the number of same color neighbors different of 1.
 * @param surf
 * @param BW
 * @param changes
 */
static void __removes_triangles_by_same_color_neighbors_count(ImpSurface * surf, ImpMatrix * BW, int * changes) {
	ListIterator * it = NULL;

	List * to_remove = list_new();

	(*changes) = 0;

	it = list_get_first(surf->faces);
	while (it) {

		ImpTriangle * T;
		int neT;

		T = IMP_TRIANGLE(it->data);
		it = it->next;

		// = Removes triangles with 0, 2 or more neighborhoods with the same color.
		neT = __count_triangle_neighbors_with_same_color(T, BW);
		if (neT != 1) {
			list_append(to_remove, T);
			(*changes)++;
			continue;
		}
	}

	it = list_get_first(to_remove);
	while (it) {

		ImpTriangle * T;
		T = IMP_TRIANGLE(it->data);
		it = it->next;

		imp_surface_remove_triangle(surf, T);
		imp_object_delete(T);
	}

	list_remove_all_data(to_remove);
	list_delete(to_remove);
}

static ImpTriangle * __get_triangle_with_same_color(ImpTriangle * T, ImpMatrix * BW) {

	ImpTriangle * t = NULL;
	ImpPoint p;
	int i;

	real color_T, color_t;

	p = imp_triangle_barycenter(T);
	color_T = BW->data[ imp_matrix_idx(BW, p.x, p.y) ];

	for (i = 0; i < 3; i++) {
		t = imp_triangle_get_neighbor(T, T->e[i]);

		if (!t) continue;

		p = imp_triangle_barycenter(t);
		color_t = BW->data[ imp_matrix_idx(BW, p.x, p.y) ];

		if (color_T == color_t) {
			return t;
		}
	}
	return NULL;
}

static int __count_triangle_neighbors_with_diferent_color(ImpTriangle * T, ImpMatrix * BW) {

	ImpTriangle * t = NULL;
	ImpPoint p;
	int i;

	real color_T, color_t;
	int nd;

	p = imp_triangle_barycenter(T);
	color_T = BW->data[ imp_matrix_idx(BW, p.x, p.y) ];


	nd = 0;
	for (i = 0; i < 3; i++) {
		t = imp_triangle_get_neighbor(T, T->e[i]);

		if (!t) continue;

		p = imp_triangle_barycenter(t);
		color_t = BW->data[ imp_matrix_idx(BW, p.x, p.y) ];

		if (color_T != color_t) nd++;
	}
	return nd;
}

/**
 * \brief Removes all triangles with the number of different color neighbors less than 2 or greater than 4.
 * @param surf
 * @param BW
 * @param changes
 */
static void __removes_triangles_by_different_color_neighbors_count(ImpSurface * surf, ImpMatrix * BW, int * changes) {
	ListIterator * it = NULL;

	List * to_remove = list_new();

	(*changes) = 0;

	it = list_get_first(surf->faces);
	while (it) {

		ImpTriangle * T;
		ImpTriangle * t;

		int ndT, ndt, nd;

		T = IMP_TRIANGLE(it->data);
		it = it->next;

		t = __get_triangle_with_same_color(T, BW);

		if (!t) {
			list_append(to_remove, T);
			(*changes)++;
			continue;
		}

		ndT = __count_triangle_neighbors_with_diferent_color(T, BW);
		ndt = __count_triangle_neighbors_with_diferent_color(t, BW);

		nd = ndT + ndt;

		if (nd < 2 || nd > 4) {
			list_append(to_remove, T);
			(*changes)++;
			continue;
		}
	}

	it = list_get_first(to_remove);
	while (it) {

		ImpTriangle * T;
		T = IMP_TRIANGLE(it->data);
		it = it->next;

		imp_surface_remove_triangle(surf, T);
		imp_object_delete(T);
	}

	list_remove_all_data(to_remove);
	list_delete(to_remove);
}

// </editor-fold>

void filt_chess(ImpSurface * surf, ImpMatrix * BW) {

	int changes = 0;

	// = Removes invalid vertexes in the surface ===============================
	__remove_invalid_points(surf, &changes);

	// = Removes invalid triangles by area =====================================
	__removes_triangles_by_area(surf, 50, 90000, &changes);

	// = Removes all triangles with color transition inside ====================
	__removes_triangles_with_color_transition(surf, BW, 0.4, &changes);

	// = Removes all triangles with same color neighbor different of 1 =========
	do {
		__removes_triangles_by_same_color_neighbors_count(surf, BW, &changes);
	} while (changes);

	// = Removes all triangles with different colors neighbor less than 2 or greater than 8
	do {
		__removes_triangles_by_different_color_neighbors_count(surf, BW, &changes);
	} while (changes);

}

// <editor-fold defaultstate="collapsed" desc="Auxiliar Create Chessboard Correspondence">

static void __get_central_triangles(ImpSurface * surf, ImpMatrix * BW, ImpTriangle ** _T1, ImpTriangle ** _T2, real * _d) {

	ImpTriangle * T1 = NULL;
	ImpTriangle * T2 = NULL;
	ImpPoint pi;
	ImpPoint pt;

	pi.x = BW->columns / 2;
	pi.y = BW->rows / 2;
	pi.z = 0.0;

	// = Get the triangle closest to image center ==============================
	ListIterator * it;
	real dt, d;
	it = list_get_first(surf->faces);

	// Initialize with big distance value.
	d = BW->columns + BW->rows;

	// Find the triangle.
	while (it) {

		pt = imp_triangle_barycenter(IMP_TRIANGLE(it->data));
		dt = imp_point_distance(&pt, &pi);

		if (dt < d) {
			d = dt;
			T1 = it->data;
		}

		it = it->next;
	}

	if (T1 == NULL) return;

	T2 = __get_triangle_with_same_color(T1, BW);

	if (T2 == NULL) return;

	*_T1 = T1;
	*_T2 = T2;
	*_d = d;
}

static real __triangle_color(ImpTriangle * T, ImpMatrix * BW) {
	ImpPoint p;
	p = imp_triangle_barycenter(T);
	return BW->data[ imp_matrix_idx(BW, p.x, p.y) ];
}

void __assing_grid_points(ImpMatrix * BW, ImpTriangle * T, ImpTriangle * Tant, ImpVertex * v, ImpVertex * vh, ImpVertex * vv, int * count) {

	_exit_if_fail(vh || vv);

	ImpEdge * eop = imp_triangle_get_opposite_edge(T, v);
	ImpTriangle * Top = imp_triangle_get_neighbor(T, eop);

	if (Top == NULL) {
		if (vh) {
			SWAP(v, vh, ImpVertex *);
		} else {
			if (vv) SWAP(v, vv, ImpVertex *);
		}
	} else {
		if (__triangle_color(T, BW) != __triangle_color(Top, BW)) {
			if (vh) {
				SWAP(v, vh, ImpVertex *);
			} else {
				if (vv) SWAP(v, vv, ImpVertex *);
			}
		}
	}

	eop = imp_triangle_get_opposite_edge(T, v);
	Top = imp_triangle_get_neighbor(T, eop);

	if (Top == NULL) {
		return;
	}

	ImpVertex * vop = imp_triangle_get_opposite_vertex(Top, eop);

	if (vop == NULL) {
		return;
	}

	// = Initialization ========================================================
	if (Tant == NULL) {

		GRID(v)->xw = 0;
		GRID(v)->yw = 0;

		GRID(vh)->xw = GRID(v)->xw + 1;
		GRID(vh)->yw = GRID(v)->yw;

		GRID(vv)->xw = GRID(v)->xw;
		GRID(vv)->yw = GRID(v)->yw + 1;

		GRID(vop)->xw = GRID(vh)->xw;
		GRID(vop)->yw = GRID(vv)->yw;

		/*
				grid[ v->id ].gx = 0;
				grid[ v->id ].gy = 0;
				grid[ vh->id ].gx = grid[ v->id ].gx + 1;
				grid[ vh->id ].gy = grid[ v->id ].gy;
				grid[ vv->id ].gx = grid[ v->id ].gx;
				grid[ vv->id ].gy = grid[ v->id ].gy + 1;
				grid[ vop->id ].gx = grid[ vh->id ].gx;
				grid[ vop->id ].gy = grid[ vv->id ].gy;
		 */
	}

	// =  ======================================================================
	ImpEdge * ex = NULL;
	ImpEdge * ey = NULL;

	ImpVertex * vaux = NULL;

	// = Computes vh  ==========================================================
	if (vh == NULL) {
		ey = imp_triangle_get_edge_that_connects_vertices(T, v, vv);
		vh = imp_triangle_get_opposite_vertex(T, ey);

		vaux = imp_triangle_get_opposite_vertex(Tant, ey);

		GRID(vh)->xw = 2 * GRID(v)->xw - GRID(vaux)->xw;
		GRID(vh)->yw = GRID(v)->yw;

		GRID(vop)->xw = GRID(vh)->xw;
		GRID(vop)->yw = GRID(vv)->yw;

		/*
				grid[ vh->id ].gx = 2 * grid[ v->id ].gx - grid[ vaux->id ].gx;
				grid[ vh->id ].gy = grid[ v->id ].gy;
				grid[ vop->id ].gx = grid[ vh->id ].gx;
				grid[ vop->id ].gy = grid[ vv->id ].gy;
		 */
	}

	// = Computes vv ===========================================================
	if (vv == NULL) {
		ex = imp_triangle_get_edge_that_connects_vertices(T, v, vh);
		vv = imp_triangle_get_opposite_vertex(T, ex);

		vaux = imp_triangle_get_opposite_vertex(Tant, ex);

		GRID(vv)->xw = GRID(v)->xw;
		GRID(vv)->yw = 2 * GRID(v)->yw - GRID(vaux)->yw;

		GRID(vop)->xw = GRID(vh)->xw;
		GRID(vop)->yw = GRID(vv)->yw;

		/*
				grid[ vv->id ].gx = grid[ v->id ].gx;
				grid[ vv->id ].gy = 2 * grid[ v->id ].gy - grid[ vaux->id ].gy;
				grid[ vop->id ].gx = grid[ vh->id ].gx;
				grid[ vop->id ].gy = grid[ vv->id ].gy;
		 */

	}

	if (!GRID(v)->visited) {
		(*count)++;
		GRID(v)->visited = TRUE;
	}
	if (!GRID(vh)->visited) {
		(*count)++;
		GRID(vh)->visited = TRUE;
	}
	if (!GRID(vv)->visited) {
		(*count)++;
		GRID(vv)->visited = TRUE;
	}
	if (!GRID(vop)->visited) {
		(*count)++;
		GRID(vop)->visited = TRUE;
	}

	// =  ======================================================================
	if (!ex) ex = imp_triangle_get_edge_that_connects_vertices(T, v, vh);
	if (!ey) ey = imp_triangle_get_edge_that_connects_vertices(T, v, vv);

	// =  ======================================================================

	T_OPT(T)->visited = TRUE;
	T_OPT(Top)->visited = TRUE;

	// =  ======================================================================
	ImpTriangle * Tx = imp_triangle_get_neighbor(T, ex);
	ImpTriangle * Ty = imp_triangle_get_neighbor(T, ey);

	ImpEdge * e = NULL;

	e = imp_triangle_get_edge_that_connects_vertices(Top, vop, vv);
	ImpTriangle * Topx = imp_triangle_get_neighbor(Top, e);

	e = imp_triangle_get_edge_that_connects_vertices(Top, vh, vop);
	ImpTriangle * Topy = imp_triangle_get_neighbor(Top, e);

	if (Tx) if (!T_OPT(Tx)->visited) __assing_grid_points(BW, Tx, T, v, vh, NULL, count);
	if (Ty) if (!T_OPT(Ty)->visited) __assing_grid_points(BW, Ty, T, v, NULL, vv, count);

	if (Topx) if (!T_OPT(Topx)->visited) __assing_grid_points(BW, Topx, Top, vop, vv, NULL, count);
	if (Topy) if (!T_OPT(Topy)->visited) __assing_grid_points(BW, Topy, Top, vop, NULL, vh, count);

}

int __compare_grid_point(void * a, void * b) {
	struct __grid_point * pa = (struct __grid_point *) a;
	struct __grid_point * pb = (struct __grid_point *) b;
	int r;

	r = pa->yw - pb->yw;
	if (r == 0) {
		r = pa->xw - pb->xw;
	}
	return r;
}

// </editor-fold>

List * create_list_correspondence(ImpSurface * surf, ImpMatrix * BW) {

	List * lgrid = NULL;

	ImpTriangle * T1 = NULL;
	ImpTriangle * T2 = NULL;

	ImpVertex * v = NULL;
	ImpEdge * es = NULL;

	real MAX_VAL = 999999;

	real d;

	if (surf->vertices->size == 0) return NULL;

	__get_central_triangles(surf, BW, &T1, &T2, &d);

	if (T1 == NULL || T2 == NULL) return NULL;

	// = Insert control structure to the triangles ============================

	ListIterator * it;
	it = list_get_first(surf->faces);
	while (it) {
		ImpTriangle * t;
		t = IMP_TRIANGLE(it->data);

		struct __triang_opt * topt;
		topt = mem_alloc(struct __triang_opt);
		topt->visited = FALSE;

		(IMP_OBJECT(it->data))->user_ptr = topt;

		it = it->next;
	}

	// = Insert control structure to the vertices ==============================
	it = list_get_first(surf->vertices);
	while (it) {
		ImpPoint * p = IMP_POINT(it->data);
		struct __grid_point * grid;

		grid = mem_alloc(struct __grid_point);
		grid->visited = FALSE;
		grid->xw = MAX_VAL; // only for post-verification
		grid->yw = MAX_VAL;

		grid->ix = p->x;
		grid->iy = p->y;

		IMP_OBJECT(it->data)->user_ptr = grid;

		it = it->next;
	}

	es = imp_triangle_get_shared_edge(T1, T2);
	v = imp_triangle_get_opposite_vertex(T1, es);

	int count = 0;
	__assing_grid_points(BW, T1, NULL, v, IMP_SEGMENT(es)->v[0], IMP_SEGMENT(es)->v[1], &count);

	lgrid = list_new();

	// = Extract coordinates correspondence ====================================
	it = list_get_first(surf->vertices);
	while (it) {

		ImpVertex * v;
		struct __grid_point * grid;

		v = IMP_VERTEX(it->data);
		it = it->next;
		grid = GRID(v);

		if (grid->visited) {



			list_insert_sorted(lgrid, grid, __compare_grid_point);

			/*
						ii = list_get_first(lgrid);
						while (ii) {
							struct __grid_point * gp = ii->data;
							printf("(%d,%d)  ", gp->xw, gp->yw);
							ii = ii->next;
						}
						printf("\n\n");
			 */
		}

		IMP_OBJECT(v)->user_ptr = NULL;
	}

	// = Remove Triangle properties ============================================
	it = list_get_first(surf->faces);
	while (it) {
		ImpTriangle * t;
		t = IMP_TRIANGLE(it->data);
		it = it->next;
		mem_free(IMP_OBJECT(t)->user_ptr);
	}

	return lgrid;
}

// =============================================================================

static List * __get_xcorners(ImpMatrix * Mat) {

	int n;

	List * lgrid = NULL;

	ImpMatrix * Corners = NULL;
	ImpMatrix * NMS = NULL;
	ImpMatrix * Label = NULL;
	ImpMatrix * Feat = NULL;

	ImpSurface * surf = NULL;

	/* = Get initial valid points =========================================== */
	n = imp_image_features_Xcorner(Mat, &Corners);
	if (n == 0) {

		imp_object_delete(Corners);
		imp_object_delete(NMS);
		imp_object_delete(Label);
		imp_object_delete(Feat);
		imp_surface_remove_all_elements_full(surf);
		imp_object_delete(surf);

		return NULL;
	}

	imp_image_nonmaxsup_feature_nonzero(Corners, 3, &NMS);
	imp_image_threshold(NMS, 0, 0, 255, IMP_THRESHOLD_BINARY, NULL);
	imp_image_morph_dilate_w(NMS, 3, &Corners);

	// avoid duplicated detection of same corner
	imp_matrix_clear(NMS);
	n = imp_image_label(Corners, &NMS, 255.0);
	imp_image_label_analisys(NMS, n, &Feat);

	imp_matrix_clear(Corners);
	for (n = 0; n < Feat->rows; n++) {
		int c = Feat->data[ n * Feat->columns + 8 ];
		int r = Feat->data[ n * Feat->columns + 9 ];
		Corners->data[ r * Corners->columns + c ] = 255;
	}

	surf = imp_delaunay_from_matrix(Corners, TRUE);

	if (surf->vertices->size == 0) {

		imp_object_delete(Corners);
		imp_object_delete(NMS);
		imp_object_delete(Label);
		imp_object_delete(Feat);
		imp_surface_remove_all_elements_full(surf);
		imp_object_delete(surf);


		return NULL;
	}

	// = Here NMS is a BW image
	imp_image_threshold_adaptive(Mat, IMP_THRESHOLD_ADAPTIVE_WELLNER_EXTENDED, &NMS);
	filt_chessboard(surf, NMS);

	imp_surface_remove_orfans_edges(surf);
	imp_surface_remove_orfans_vertices(surf);

	if (surf->vertices->size == 0) {

		imp_object_delete(Corners);
		imp_object_delete(NMS);
		imp_object_delete(Label);
		imp_object_delete(Feat);
		imp_surface_remove_all_elements_full(surf);
		imp_object_delete(surf);

		return NULL;
	}

	lgrid = create_list_correspondence(surf, NMS);

	imp_object_delete(Corners);
	imp_object_delete(NMS);
	imp_object_delete(Label);
	imp_object_delete(Feat);
	imp_surface_remove_all_elements_full(surf);
	imp_object_delete(surf);

	return lgrid;

}

static void __update2best_inregion(ImpMatrix * Hresp, real * _x, real * _y, int w) {

	int rows = Hresp->rows;
	int columns = Hresp->columns;
	int r, c, _r, i;

	int x = *_x;
	int y = *_y;

	real MIN = Hresp->data[ x + y * columns];

	for (r = y - w, _r = r * columns; r < y + w; r++, _r += columns) {

		if (0 >= r || r >= rows) continue;

		for (c = x - w; c < x + w; c++) {
			i = _r + c;

			if (0 >= c || c >= columns) continue;

			if (Hresp->data[i] < MIN) {
				MIN = Hresp->data[i];
				*_x = c;
				*_y = r;
			}
		}
	}
}

static inline void __update2sub_pixel(ImpMatrix * Dx, ImpMatrix * Dy, ImpMatrix * Dxy, ImpMatrix * Dxx, ImpMatrix * Dyy, real * x, real * y) {

	int i = (*x) + (*y) * Dx->columns;

	real rx = Dx->data[i];
	real ry = Dy->data[i];
	real rxy = Dxy->data[i];
	real rxx = Dxx->data[i];
	real ryy = Dyy->data[i];

	// calculates sub-pixel
	real den = (rxx * ryy - rxy * rxy);
	real s = (ry * rxy - rx * ryy) / den;
	real t = (rx * rxy - ry * rxx) / den;

	(*x) += s;
	(*y) += t;
}

static void __extract_matrices_from_list(List * lgrid, ImpMatrix ** Xw, ImpMatrix ** Xi) {

	int i, d;
	ListIterator * it;

	// = Convert grid to Matrix ================================================
	*Xw = imp_matrix_new(2, lgrid->size);
	*Xi = imp_matrix_new(2, lgrid->size);

	i = 0;
	d = (*Xi)->columns;

	it = list_get_first(lgrid);
	while (it) {

		struct __grid_point * grid = (struct __grid_point *) it->data;

		(*Xw)->data[i + 0] = grid->xw;
		(*Xw)->data[i + d] = grid->yw;

		(*Xi)->data[i + 0] = grid->ix;
		(*Xi)->data[i + d] = grid->iy;

		it = it->next;
		i++;
	}
}

static void __get_search_grid(ImpMatrix * pXr, int width, int height, ImpMatrix * H) {

	int i;
	ImpMatrix * sXi = NULL; // search grid in image
	real * h = H->data;

	real minx, maxx;
	real miny, maxy;

	// = Get max and min at x and y coordinates ================================
	minx = maxx = pXr->data[0];
	miny = maxy = pXr->data[ pXr->columns ];
	for (i = 0; i < pXr->columns; i++) {


		if (MAT(pXr, 0, i) < minx) minx = MAT(pXr, 0, i);
		if (MAT(pXr, 0, i) > maxx) maxx = MAT(pXr, 0, i);

		if (MAT(pXr, 1, i) < miny) miny = MAT(pXr, 1, i);
		if (MAT(pXr, 1, i) > maxy) maxy = MAT(pXr, 1, i);

		if (pXr->data[i] < minx) minx = pXr->data[i];
		if (pXr->data[i] > maxx) maxx = pXr->data[i];

		if (pXr->data[i + pXr->columns] < miny) miny = pXr->data[i + pXr->columns];
		if (pXr->data[i + pXr->columns] > maxy) maxy = pXr->data[i + pXr->columns];
	}


	// = Calculates size of input rectangle
	int sx = (maxx - minx) + 1;
	int sy = (maxy - miny) + 1;

	if (width < height) {

		if (sy > width) {
			// ok
		} else if (sx > width) {
			// should invert coordinates
			//SWAP(sx, sy, int);
			SWAP(width, height, int);
		} else {
			// undetermined
			if (width > height) height = width;
			else width = height;
		}

	} else if (height < width) {

		if (sx > height) {
			// ok
		} else if (sy > height) {
			// should invert coordinates
			//SWAP(sx, sy, int);
			SWAP(width, height, int);
		} else {
			// undetermined
			if (width > height) height = width;
			else width = height;
		}

	} else {
		// width == height
	}

	// = Calculates size of grid search
	int gx = 2 * width - sx; // = (x-sx)*2 + sx;
	int gy = 2 * height - sy; // = (y-sy)*2 + sy;

	int sz = gx*gy;

	// = Get search grid in coordinate image space =============================
	sXi = imp_matrix_new(2, sz);

	int cx = (width - sx) - minx;
	int cy = (height - sy) - miny;

	int m, n;
	i = 0;
	for (m = 0; m < gy; m++) {
		for (n = 0; n < gx; n++) {

			real xr = n - cx;
			real yr = m - cy;
			real zr = 1;

			real xi = h[0] * xr + h[1] * yr + h[2] * zr;
			real yi = h[3] * xr + h[4] * yr + h[5] * zr;
			real zi = h[6] * xr + h[7] * yr + h[8] * zr;

			sXi->data[ i ] = xi / zi;
			sXi->data[ i + sz ] = yi / zi;

			i++;

		}
	}

	//	return sXi;
}

static void __fill_grid(int minx, int maxx, int miny, int maxy, List * lgrid, ImpMatrix * Hresp, ImpMatrix * H, ImpMatrix * Dx, ImpMatrix * Dy, ImpMatrix * Dxy, ImpMatrix * Dxx, ImpMatrix * Dyy) {

	ListIterator * it = NULL;
	real * h = H->data;

	int xi, yi;
	int xf, yf;

	struct __grid_point * grid = NULL;

	it = list_get_first(lgrid);

	for (yi = miny; yi <= maxy; yi++) {
		for (xi = minx; xi <= maxx; xi++) {

			if (it != NULL) {
				grid = (struct __grid_point *) it->data;
				xf = grid->xw;
				yf = grid->yw;
			} else {
				xf = maxx;
				yf = maxy;
			}

			//printf("== (%d,%d) --> (%d,%d) ===\n", xi, yi, xf, yf);

			int idi = (xi - minx) + (yi - miny)*(maxx - minx + 1);
			int idf = (xf - minx) + (yf - miny)*(maxx - minx + 1);

			if (idi < idf) {

				//printf("Inserting (%d,%d)\n", xi, yi);

				struct __grid_point * new_grid = mem_alloc(struct __grid_point);

				real _x, _y, _z;
				_x = h[0] * xi + h[1] * yi + h[2] * 1;
				_y = h[3] * xi + h[4] * yi + h[5] * 1;
				_z = h[6] * xi + h[7] * yi + h[8] * 1;

				new_grid->ix = _x / _z;
				new_grid->iy = _y / _z;

				__update2best_inregion(Hresp, &new_grid->ix, &new_grid->iy, 1);
				__update2sub_pixel(Dx, Dy, Dxy, Dxx, Dyy, &new_grid->ix, &new_grid->iy);

				list_insert_sorted(lgrid, new_grid, __compare_grid_point);

			} else {
				if (it) it = it->next;
			}
		}

	}

}

boolean chess_detect(ImpMatrix * Mat, int w, int h, ImpMatrix ** _Xi, ImpMatrix ** _Xw) {

	int n;
	int i;

	ImpMatrix * Xi;
	ImpMatrix * Xw;

	ImpMatrix * Hresp = NULL;
	ImpMatrix * H = NULL;

	List * lgrid = NULL;

	/* = Step 01: Get initial valid points ================================== */
	lgrid = __get_xcorners(Mat);
	if (lgrid == NULL) return FALSE;

	/* = Step 02: Calculates Hresp ========================================== */
	ImpMatrix * Dx = NULL;
	ImpMatrix * Dy = NULL;
	ImpMatrix * Dxy = NULL;
	ImpMatrix * Dxx = NULL;
	ImpMatrix * Dyy = NULL;

	imp_matrix_derivative(Mat, &Dx, &Dy, &Dxy);
	imp_matrix_derivative2(Mat, &Dxx, &Dyy);

        Hresp = imp_matrix_new(Mat->rows, Mat->columns);
        
	for (i = 0; i < Mat->size; i++) {
		Hresp->data[i] = (Dxx->data[i] * Dyy->data[i]) - (Dxy->data[i] * Dxy->data[i]);
	}

	/* = Step 03: Update points to best Hresp and to sup-pixel precision  */
	ListIterator * it;
	it = list_get_first(lgrid);

	while (it) {
		struct __grid_point * grid = (struct __grid_point *) it->data;

		__update2best_inregion(Hresp, &grid->ix, &grid->iy, 2);
		__update2sub_pixel(Dx, Dy, Dxy, Dxx, Dyy, &grid->ix, &grid->iy);

		it = it->next;
	}

	/* = Step 04: Get the search grid ======================================= */
	//real minx, maxx;
	//real miny, maxy;

	//__extract_matrices_from_list(lgrid, &Xw, &Xi);

	//__vminmax(Xw->columns, &Xw->data[0], 1, &minx, &maxx);
	//__vminmax(Xw->columns, &Xw->data[ Xw->columns ], 1, &miny, &maxy);

	//nDLT(Xw, Xi, &H);

	//__fill_grid(minx, maxx, miny, maxy, lgrid, Hresp, H, Dx, Dy, Dxy, Dxx, Dyy);

	__extract_matrices_from_list(lgrid, _Xw, _Xi);



	imp_object_delete(Xi);
	imp_object_delete(Xw);
	imp_object_delete(Hresp);
	imp_object_delete(H);

	list_remove_all_data_full(lgrid, free);
	list_delete(lgrid);

	imp_object_delete(Dx);
	imp_object_delete(Dy);
	imp_object_delete(Dxy);
	imp_object_delete(Dxx);
	imp_object_delete(Dyy);


	return TRUE;

	/* = Step 01: Get initial regions of valid points */

	/* = Step 02: Get the max Hresp in each region of valid points */

	/* = Step 03: Update them to sup-pixel precision and set they as valid points */

	/* = Step 04: Compute Homographic transformation */

	/* = Step 05: Calculates the fit of homographic transformation with the valid points */

	/* = Step 06: Estimate a new region point close to the grid with the homographic transformation */

	/* = Step 07: Get the max Hresp of new region */

	/* = Step 08: Update the new point to sub-pixel precision */

	/* = Step 09: Compute homographic transformation with the new point */

	/* = Step 10: Calculates the fit of new homographic transformation with the valid points */

	/* = Step 11: If the fit is best than the last fit, insert a new point to set of valid points */

	/* = Step 12: If the number of valid points is not ok then go to step 06 else end */

}

